#include "openniGraber.h"
#include "yamlConfig.h"

depthCapture::depthCapture(DEVICEID ID)
{
    //初始化openni
    if(openni::OpenNI::initialize() != openni::STATUS_OK)
    { 
        std::cerr << "OpenNI Initial Error: " << openni::OpenNI::getExtendedError() << std::endl; 
        this->is_opened = false;
    }

    //从yaml配置文件中获取设备id
    deviceId = g_camre_config.getDeviceId(ID);
    //先获取id再由id读取实际的设备uri
    getDeviceUri();

    //打开设备
    if(mDevice.open(deviceUri.c_str()) != openni::STATUS_OK) 
    { 
        std::cerr << "Can't Open Device: " << openni::OpenNI::getExtendedError() << std::endl; 
        this->is_opened = false; 
    } 
    //创建深度数据流
    if(mDevice.hasSensor(openni::SENSOR_DEPTH))
    {
        if(mDepthStream.create(mDevice, openni::SENSOR_DEPTH) == openni::STATUS_OK)
        {
            openni::VideoMode mMode;
            mMode.setResolution(g_camre_config.resolution[0], g_camre_config.resolution[1]);
            mMode.setFps(g_camre_config.fps);
            mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);

            if(mDepthStream.setVideoMode(mMode) != openni::STATUS_OK)
            {
                //初始化深度视频格式失败
                std::cerr << "Cannot apply video mode to depth stream, check your config file." << "\n"
                          << openni::OpenNI::getExtendedError() << std::endl;
                this->is_opened = false;
            }
        }else
        {
            //打开设备失败
            std::cerr << "Cannot creat depth stream on device: " << openni::OpenNI::getExtendedError() << std::endl;
            this->is_opened = false;
        }
        //打开设备
        mDepthStream.start();
    }
    else
    {
        //设备不具备深度传感器
        std::cerr << "ERROR: This device does not have depth sensor" << std::endl;
        this->is_opened = false;
    }
    this->is_opened = true;
}

//设备id是不变的，从设备id解算设备uri
std::string depthCapture::getDeviceUri()
{
    openni::Array<openni::DeviceInfo> deviceInfo;
    openni::OpenNI::enumerateDevices(&deviceInfo);
    for(int i=0;i<deviceInfo.getSize();i++)
    {
        const openni::DeviceInfo& eachDeviceInfo = deviceInfo[i];
        if(deviceInfo[i].getUsbProductId() == this->deviceId)
            this->deviceUri = eachDeviceInfo.getUri();
    }
}

bool depthCapture::shutdown()
{
    mDepthStream.destroy();
    mDevice.close();
    openni::OpenNI::shutdown();
    return true;
}

bool depthCapture::getCloudXYZCoordinate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_XYZ)
{
    openni::VideoFrameRef mDepthFrame; 
    if(mDepthStream.readFrame(&mDepthFrame) == openni::STATUS_OK) 
    { 
        float fx,fy,fz; 
        int i=0; //以米为单位 
        double fScale = 0.001; 
        openni::DepthPixel *pDepthArray = (openni::DepthPixel*)mDepthFrame.getData(); 
        for(int y = 0; y < mDepthFrame.getHeight(); y++) 
        { 
            for(int x = 0; x < mDepthFrame.getWidth(); x++) 
            { 
                int idx = x + y*mDepthFrame.getWidth(); 
                const openni::DepthPixel rDepth = pDepthArray[idx]; 
                openni::CoordinateConverter::convertDepthToWorld(mDepthStream,x,y,rDepth,&fx,&fy,&fz); 
                fx = -fx; fy = -fy; 
                cloud_XYZ->points[i].x = fx * fScale; 
                cloud_XYZ->points[i].y = fy * fScale; 
                cloud_XYZ->points[i].z = fz * fScale; 
                i++; 
            } 
        } 
        return true; 
    } else 
    { 
        std::cout << "getCloudXYZCoordinate: fail to read frame from depth stream" << std::endl; 
        return false; 
    } 
}

bool depthCapture::isOpened()
{
    return this->is_opened;
}

void detectDevices()
{
    //初始化openni
    if(openni::OpenNI::initialize() != openni::STATUS_OK)
    {
        std::cerr << "OpenNI Initial Error: " << openni::OpenNI::getExtendedError() << std::endl; 
        return;
    }
    openni::Array<openni::DeviceInfo> deviceInfo;
    openni::OpenNI::enumerateDevices(&deviceInfo);
    std::cout << "Detected " << deviceInfo.getSize() << " openni grabers" <<std::endl;
    for(int i=0;i<deviceInfo.getSize();i++)
    {
        const openni::DeviceInfo& eachDeviceInfo = deviceInfo[i];
        std::cout << "Grabber:      " << i << "\n"
                  << "Name:         " << eachDeviceInfo.getName() << "\n"
                  << "Id:           " << eachDeviceInfo.getUsbProductId() << "\n"
                  << "Uri:          " << eachDeviceInfo.getUri() << std::endl;
    }
}

void depthCapture::operator >> (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_XYZ)
{
    //将深度数据流中的一帧转换为点云视频的一帧
    this->getCloudXYZCoordinate(cloud_XYZ);
}